#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Sensor, S2,     armtouch,       sensorTouch)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Touch,          sensorTouch)
#pragma config(Motor,  motorA,          harvesterL,    tmotorNormal, openLoop)
#pragma config(Motor,  motorB,          harvesterR,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  motorC,          ,              tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     motorR,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorL,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     elbow,         tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     harvesterlift, tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void resetarm()
{

  while (SensorValue(armtouch) == 0)
  {
    motor[elbow]= -10;
  }
  motor [elbow]=-5;
  wait1Msec (50);
  motor [elbow]=0;
  wait1Msec (50);
  return;
}
task main()
{
  resetarm();

  nMotorEncoder[elbow] = 0;


  while (nMotorEncoder[elbow] < 1250)
  {
    motor[elbow] = 10;

  }

  motor[elbow] = 0; //turn both TETRIX motors off
  while (nMotorEncoder [elbow] > 1250)
  {
    motor [elbow]=-15;
  }
  motor[elbow]=0;

  wait1Msec(1500);

  while (nMotorEncoder[elbow] > 500)
  {
    motor [elbow] = -30;
  }
  motor [elbow] = 0;
  wait1Msec (1000);
}
